/**
 * @file omni_wheel_cal.c
 * @author lwz (3296626485@qq.com)
 * @version 2025 BNGU
 * @date 2025-04-13
 */

#include "main.h"
#include "math.h"
#include "omni_wheel_cal.h"

 /*
 *********************************************************************************************************
 *                                            	变量定义
 *********************************************************************************************************
 */
const float Wheel_R=1;
const float Robot_R=1;


/*
*********************************************************************************************************
*                                               函数实现
*********************************************************************************************************
*/

//在世界坐标系下，速度分量 Vx和Vy分别表示机器人在全局坐标系的 x 轴和 y 轴方向上的速度。
//而在自身坐标系下，速度分量通常表示为机器人前进方向（通常是 x 轴）和横向方向（通常是 y 轴）的速度。
void Robot_Velocity_To_Wheel_Speed(Wheel_Data* robot) // 速度分解
{
	float Vx,Vy,Vw;
    Vy = -robot->Robot_in_self.velocity_exp.Vx;
    Vx = robot->Robot_in_self.velocity_exp.Vy;
    Vw = robot->Robot_in_self.velocity_exp.Vw* Robot_R;
    

    robot->Wheel_Speed[0] = (sqrt(2) / 2) * Vx + (-sqrt(2) / 2) * Vy - Vw * Robot_R;
    robot->Wheel_Speed[1] = (sqrt(2) / 2) * Vx + (sqrt(2) / 2) * Vy - Vw * Robot_R;
    robot->Wheel_Speed[2] = (-sqrt(2) / 2) * Vx + (sqrt(2) / 2) * Vy - Vw * Robot_R;
    robot->Wheel_Speed[3] = (-sqrt(2) / 2) * Vx + (-sqrt(2) / 2) * Vy - Vw * Robot_R;
}

//机器人世界坐标系转自身坐标系速度
void World_To_Self(Wheel_Data* robot)
{
    float Vx, Vy, YAW;
    YAW = robot->Robot_in_world.position_exp.yaw * PI / 180;
    Vx = robot->Robot_in_world.velocity_exp.Vx;
    Vy = robot->Robot_in_world.velocity_exp.Vy;
    robot->Robot_in_self.velocity_exp.Vx = Vx * cosf(YAW) - Vy * sinf(YAW);
    robot->Robot_in_self.velocity_exp.Vy = Vx * sinf(YAW) + Vy * cosf(YAW);
	robot->Robot_in_self.velocity_exp.Vw = robot->Robot_in_world.velocity_exp.Vw;
}

void Wheel_Data_To_Motor(Wheel_Data* robot,DJI_Motor_ControlTypeDef* motor)
{
    for (int i = 0; i < 4; i++)
    {
        motor->M3508[i].exp_speed = (int16_t)robot->Wheel_Speed[i];
    }
}
